#include <serial/serial.h>
#include<iostream>

#define GET_LOW_BYTE(A) (uint8_t)((A))
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))

#define SET_SPEED 10
#define SET_SWING 12

class moveCom
{
public:
    moveCom(){};
    ~moveCom(){};
    int moveComOpen(std::string movePort){
        try{
            (this->ser).setPort(movePort);  //设置端口号
            (this->ser).setBaudrate(115200);  //设置波特率
            serial::Timeout tout = serial::Timeout::simpleTimeout(2000);//设置延时等待 ms
            (this->ser).setTimeout(tout); 
            (this->ser).open(); //打开串
        }
        catch (serial::IOException& e) {
            std::cout<<"movePort is not open"<<std::endl;
            return 0; 
        } 
        if((this->ser).isOpen()){
            std::cout<<"movePort is open"<<std::endl;
            return 1;
        } 
        else {
            std::cout<<"movePort is not open"<<std::endl;
            return 0; 
        } 
    }
    void setSpeed(uint16_t x_speed,uint16_t y_speed,uint16_t w_speed){
        unsigned char buf[8];
        buf[0]=SET_SPEED;
        buf[1]=GET_HIGH_BYTE(x_speed);
        buf[2]=GET_LOW_BYTE(x_speed);
        buf[3]=GET_HIGH_BYTE(y_speed);
        buf[4]=GET_LOW_BYTE(y_speed);
        buf[5]=GET_HIGH_BYTE(w_speed);
        buf[6]=GET_LOW_BYTE(w_speed);
        buf[7]=LobotCheckSum(buf,8);
        this->ser.write(buf, 8);
    }
    void setSwing(unsigned char id,unsigned char state){
        unsigned char buf[4];
        buf[0]=SET_SWING;
        buf[1]=id;
        buf[2]=state;
        buf[3]=LobotCheckSum(buf,4);
        this->ser.write(buf, 4);
    }
private:
    serial::Serial ser;
    unsigned char LobotCheckSum(unsigned char data[],int Len){
        unsigned char check;
        check=data[0];
        unsigned char ret;
        for(int i=1;i<Len-1;i++){
            check=check^data[i];
        }
        return check;
    }
};
